#ifndef __FP_H__
#define __FP_H__

#include <inttypes.h> // <<<
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <avr/wdt.h>
#include <avr/io.h>    
#include <avr/pgmspace.h>
#include <avr/eeprom.h>
#include <avr/interrupt.h>
#include <avr/sleep.h>
#include <util/delay.h>
#include <avr/eeprom.h>
#include <avr/sfr_defs.h>
#include <avr/wdt.h>
#include <math.h>
// >>>

#include "usart.h"
#include "irdecoder.h"
#include "quadpod.h" // include common configuration for the quadpod.


#if defined(FLOATINGPOINT_MATH) && defined(FIXEDPOINT_MATH)
#error Use either FIXEDPOINT or FLOATINGPOINT math. You cannot use both in paralel.
#endif

#ifdef FIXEDPOINT_MATH // <<<
#include <fixmath.h>

#if defined(ADVANCED_SWAY)
#define XTABLE_X_SIZE					0x12 
#else
#define XTABLE_X_SIZE					0xC
#endif

#define MIN_ANGLE_INDEX					0x0
#define MAX_ANGLE_INDEX					0xC
#define INIT_ANGLE_INDEX  				0x6

#define F16_COXA							0x000ECCCC  /* Fix16 value of Coxa (14.8mm) */
#define F16_FEMUR							0x002D7333  /* Fix16 value of Femur (45.45mm) */
#define F16_TIBIA							0x003B4A3D  /* Fix16 value of Tibia (59.29mm) */
#define F16_BASE							0x005C0000  /* Fix16 value of Base (92mm) */
#define F16_TWO_FEMUR					0x005AE667  /* Fix16 value of 2*Femur (45.45mm) */
#define F16_TWO_FEMUR_TIBIA			0x150D7604  /* Fix16 value of Femur*Tibia*2 */
#define F16_TIBIA_SQ						0x0DBB4CCC  /* Fix16 value of Tibia^2 */
#define F16_FEMUR_SQ						0x0811B333  /* Fix16 value of Femur^2 */
#define F16_SUM_TIBIA_SQ_FEMUR_SQ	0x15CD01B0  /* Fix16 value of Tibia^2 + Femur^2 */

#define F16_2_PI							0x0006487E  /* Fix16 value of 2*PI */
#define F16_PI 							0x0003243F  /* Fix16 value of PI */
#define fix16_minus_one					0xFFFF0000  /* Fix16 value of -1*/

#define fix16_clock_div_8k				0x07333333
/* ATTENTION: this value is clock and prescaller relevant. It is claculated
 * as:
 *
 * 	F_CPU / 1000 / PRESCALER
 *
 * where value 0x07333333 is for clock speed 14.7456 MHz and prescaller = 8
 */

#endif // >>>
#ifdef FLOATINGPOINT_MATH // <<<

#define COXA								14.8      /* double value of Coxa (14.8mm) */
#define FEMUR								45.45     /* double value of Femur (45.45mm) */
#define TIBIA								59.29     /* double value of Tibia (59.29mm) */
#define BASE								92 	    /* double value of Base (92mm) */
#define TWO_FEMUR_TIBIA					5389.461  /* double value of 2*Femur*Tibia  */
#define TIBIA_SQ							3515.30   /* double value of Tibia^2 */
#define FEMUR_SQ							2065.70   /* double value of Femur^2 */
#define SUM_TIBIA_SQ_FEMUR_SQ			5581.0066 /* double value of Tibia^2 + Femur^2 */

#endif //>>>

#if defined(FIXEDPOINT_MATH) || defined(FLOATINGPOINT_MATH)
enum servo_reversal_t {
	NORMAL = 0,
	INVERT
};
typedef enum servo_reversal_t servo_reversal_t;


struct angles_t {
	uint16_t		alpha;
	uint16_t		beta;
	uint16_t		gamma;
};
typedef struct angles_t angles_t;
#endif

#if defined(FIXEDPOINT_MATH)

struct f16_vector_t {
	fix16_t	x;
	fix16_t	y;
	fix16_t	z;
};
typedef struct f16_vector_t f16_vector_t;


struct f16_vector_short_t {
	fix16_t	x;
	fix16_t	y;
};
typedef struct f16_vector_short_t f16_vector_short_t;

struct f16_gait_type_t {
	f16_vector_short_t arm[4];
};
typedef struct f16_gait_type_t f16_gait_type_t; 


   /***********************/
   /* FUNCTION PROTOTYPES */
   /***********************/

uint16_t f16_servo_ctrl ( fix16_t angle, servo_reversal_t invert );
/* The function converts the angle into control pulse for specific
 * servo. The function requires two constants for the linear regression.
 *
 * ATTENTION: The linear regression coeficients A,B  are servo dependant. 
 * The dependence between the width of the control pulse and the angle is 
 * considered linear. Therefore a linear equation is used to determine 
 * the width of control pulse for a servo based on a known angle. 
 * The linear equation is: 
 *
 * 	y = Ax+B
 *
 * where x is the angle in radians, A,B are linear constants and y is width
 * of the control pulse in milliseconds. The used servos (TowerPro 9g) has the
 * parameters:
 *
 * 	A=0.6364 :( 0x0000A2EB in Fixed point representation)
 * 	B=0.4524 :( 0x000073D1 in fixed point representation)
 *
 * Both values are defined:
 */

#define fix16_servo_a					0x0000A2EB
#define fix16_servo_b					0x000073D1


angles_t f16_p2a( f16_vector_t * v );
/* Converts the point in [x,y,z] system into angles [A,B,C] by using
 * the known physical dimensions of the arm.
 *
 *             /\
 *            /  \
 *           / B  \
 *          /      \ FEMUR [45.45 mm]     A
 *         /        \                     | (z)
 *  TIBIA /          \                    |
 * [59.3 mm]       A1 \__________Gamma (rotate)
 *      /          . '|    COXA [14.8 mm] |
 *     /       . ' A0                     |
 *    /     .j'       |
 *   /  . '            HEIGTH [92 mm]    ^
 *  /. '              |                   \
 * -X- - - - - - - - - - - - - - - (x <->) \ (y=0)
 *  SP[51,51,51]                            \
 *
 * Angle Gamma is the rotation of the whole arm around the shoulder
 * pivot.
 *
 *	j = hypot(x,y)-coxa;
 *
 *	G=atan(y/x);
 *	A0=atan(j/z);
 *	A1=acos((femur^2+j^2+z^2-tibia^2)/(2*hypot(j,z)*femur));
 *	B=acos((tibia^2+femur^2-z^2-j^2)/(2*femur*tibia));
*/


fix16_t f16_step( fix16_t step, uint8_t arm, uint8_t k );
/* This robot uses the staticaly balanced crawling gait.
 * the sequence of lifting and placing the arms on the 
 * walking surface follows the pattern 1,3,2,4. 
 */

fix16_t f16_sway( fix16_t step, uint8_t stepper_current );
/* Swaying pattern 
 * Swaying movement immitated the shifting of CoG - lizard-like movement.
 * This movement increases the stability margin for statically balanced
 * crawling gait. The implemented schema works in combination with 
 * the gait 1,3,2,4 decsribed  in the function f16_step.
 *
 * Overview:
 *
 *     1       2
 *     +       +
 *       +---+
 *       | X |
 *       +---+
 *     +       +
 *     4       3
 *
 *   + - sweet point aka mid of the step trajectory for all 4 arms.
 *   X - Centre of Gravity (CoG) of the robor.
 *
 * where the arms travel along the pattern:
 *
 * FORWARD:
 *
 *                   (top)
 *      1         2         3         4
 *    
 *      |  /   \  |         /         o
 *      | /     \ |        /|         |\
 *      |/       \|       / |         | \
 *      /         o       \ |         | /
 *    -/+-       -+\-     -\+-       -+/-
 *    / |         | \       o         /
 *    \ |         | /       |\       /|
 *     \|         |/        | \     / |
 *      o         /         |  \   /  |
 *
 *                 (bottom)
 *
 * REVERSE:
 *
 *                   (top)
 *      1         2         3         4
 *    
 *   \  |         |  /      o         /     
 *    \ |         | /       |\       /|     
 *     \|         |/        | \     / |     
 *      o         /         | /     \ |     
 *     -+\-     -/+-       -+/-     -\+-    
 *      | \     / |         /         o     
 *      | /     \ |        /|         |\    
 *      |/       \|       / |         | \   
 *      /         o      /  |         |  \  
 *       
 *                 (bottom)
 *
 * where the ideal trajectory is the straight line and the lizard
 * like movement documents the curved path. The path for arms can
 * only work in combination with the gait 1-3-2-4. The initial positions 
 * for all arms are indicated in the drawing by the 'o' symbol. The 
 * symbol '-+-' indicates the mid of the trajectory of a step for
 * each arm. The mid of the trajectory is indicated in the overview
 * drawing above. Changing the fromward and reverse movement can happen
 * when the trajectory crosses the axis x - movement trajectory. 
 */

#ifdef FIXMATH_FULL_LIBRARY
f16_vector_t f16_rotate_xy( f16_vector_t * v, fix16_t angle );
f16_vector_t f16_rotate_y( f16_vector_t * v, fix16_t angle );
#endif
f16_vector_t f16_rotate_m( f16_vector_t * v, fix16_t * m );
/* Rotation function.
 * Rotate vector v by rotation matrix m. The matrix has elements:
 *
 *    | cos(G) , -sin(G) |
 *    | sin(G) ,  cos(G) |
 *
 * Where the matrix is passed as one dimensional array. First 
 * goes first row following that goes the secord row.
 *
 * */

#endif

#if defined(FLOATINGPOINT_MATH)

struct _vector_t {
	double	x;
	double	y;
	double	z;
};
typedef struct _vector_t vector_t;

   /***********************/
   /* FUNCTION PROTOTYPES */
   /***********************/

uint16_t servo_ctrl ( double angle );
angles_t p2a( vector_t * v );

#endif

#endif // __FP_H__
